/*
 *  V4L2 video capture example
 *
 *  This program can be used and distributed without restrictions.
 *
 *
 *  the command play it .

 AUTHOR: kzjeef@gmail.com
 DATE  : 2008.11.23
 *
 cat /tmp/test.yuv |		  \
 yuv4mpeg -w 320 -h 240 -x 422  | \
 yuv4mpeg -w 320 -h 240 -r 7:7 -x 420jpeg > ~/test.mpeg 

*/

#include "capture.h"

void process_image  (const void* , size_t );

typedef enum {
    IO_METHOD_READ,
    IO_METHOD_MMAP,
    IO_METHOD_USERPTR,
} io_method;

struct buffer {
    void *                  start;
    size_t                  length;
};

static char *           dev_name        = NULL;
static io_method	io		= IO_METHOD_MMAP;
static int              fd              = -1;
struct buffer *         buffers         = NULL;
static unsigned int     n_buffers       = 0;
static struct timeval	start_time;
static int		_camera_framerate = 15;
static int		enum_all	= 0;
int		verbose			= 0;
int		_width			= 0;
int		_height			= 0;
int		_quality		= 100; /* default jpeg
					* quality */
int		test_file_fd		= 0;
int		output_jpeg		= 0;
int		direct_rgb		= 0;
int		dry_run			= 0;
char		*fb_dev			= 0;



static void
errno_exit                      (const char *           s)
{
    fprintf (stderr, "%s error %d, %s\n",
	     s, errno, strerror (errno));

    exit (EXIT_FAILURE);
}

static int
xioctl                          (int                    fd,
                                 int                    request,
                                 void *                 arg)
{
    int r;

    do r = ioctl (fd, request, arg);
    while (-1 == r && EINTR == errno);

    return r;
}


static int
read_frame			(void)
{
    struct v4l2_buffer buf;
    unsigned int i;

    switch (io) {
    case IO_METHOD_READ:
	if (-1 == read (fd, buffers[0].start, buffers[0].length)) {
	    switch (errno) {
	    case EAGAIN:
		return 0;

	    case EIO:
		/* Could ignore EIO, see spec. */

		/* fall through */

	    default:
		errno_exit ("read");
	    }
	}

	process_image (buffers[0].start, buffers[0].length);

	break;

    case IO_METHOD_MMAP:
	CLEAR (buf);

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;

	if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
	    switch (errno) {
	    case EAGAIN:
		return 0;

	    case EIO:
		/* Could ignore EIO, see spec. */

		/* fall through */

	    default:
		errno_exit ("VIDIOC_DQBUF");
	    }
	}

	assert (buf.index < n_buffers);

	process_image (buffers[buf.index].start, buffers[buf.index].length);

	if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
	    errno_exit ("VIDIOC_QBUF");

	break;

    case IO_METHOD_USERPTR:
	CLEAR (buf);

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_USERPTR;

	if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
	    switch (errno) {
	    case EAGAIN:
		return 0;

	    case EIO:
		/* Could ignore EIO, see spec. */

		/* fall through */

	    default:
		errno_exit ("VIDIOC_DQBUF");
	    }
	}

	for (i = 0; i < n_buffers; ++i)
	    if (buf.m.userptr == (unsigned long) buffers[i].start
		&& buf.length == buffers[i].length)
		break;

	printf("buf[%2d]:%p , %d buf:%p, %d\n",
	       i,
	       buffers[i].start,
	       buffers[i].length,
	       buf.m.userptr,
	       buf.length);
	assert (i < n_buffers);
	assert (buf.m.userptr != 0);
	assert (buf.length != 0);
	
	process_image ((void *) buf.m.userptr, buf.length);

	if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
	    errno_exit ("VIDIOC_QBUF");

	break;
    }
    if (verbose)  {
	static int frame_count = 0;
	frame_count++;
	struct timeval cur;
	gettimeofday(&cur , NULL);
	printf("frame rate %ld \n",
	       frame_count
	       / (cur.tv_sec+1 - start_time.tv_sec));
    }

    return 1;
}

static void
mainloop                        (void)
{
    unsigned int count;

    count = 20;

    while (count-- > 0) {
	for (;;) {
	    fd_set fds;
	    struct timeval tv;
	    int r;

	    FD_ZERO (&fds);
	    FD_SET (fd, &fds);

	    /* Timeout. */
	    tv.tv_sec = 2;
	    tv.tv_usec = 0;

	    r = select (fd + 1, &fds, NULL, NULL, &tv);

	    if (-1 == r) {
		if (EINTR == errno)
		    continue;

		errno_exit ("select");
	    }

	    if (0 == r) {
		fprintf (stderr, "select timeout\n");
		exit (EXIT_FAILURE);
	    }

	    if (read_frame ())
		break;
	
	    /* EAGAIN - continue select loop. */
	}
    }
}

static void
stop_capturing                  (void)
{
    enum v4l2_buf_type type;

    switch (io) {
    case IO_METHOD_READ:
	/* Nothing to do. */
	break;

    case IO_METHOD_MMAP:
    case IO_METHOD_USERPTR:
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
	    errno_exit ("VIDIOC_STREAMOFF");

	break;
    }
}


static void
start_capturing                 (void)
{
    unsigned int i;
    enum v4l2_buf_type type;

    switch (io) {
    case IO_METHOD_READ:
	/* Nothing to do. */
	break;

    case IO_METHOD_MMAP:
	for (i = 0; i < n_buffers; ++i) {
	    struct v4l2_buffer buf;

	    CLEAR (buf);

	    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    buf.memory      = V4L2_MEMORY_MMAP;
	    buf.index       = i;

	    if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
		errno_exit ("VIDIOC_QBUF");
	}
		
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		
	if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
	    errno_exit ("VIDIOC_STREAMON");

	break;

    case IO_METHOD_USERPTR:
	for (i = 0; i < n_buffers; ++i) {
	    struct v4l2_buffer buf;

	    CLEAR (buf);

	    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    buf.memory      = V4L2_MEMORY_USERPTR;
	    buf.index       = i;
	    buf.m.userptr	= (unsigned long) buffers[i].start;
	    buf.length      = buffers[i].length;

	    if(verbose) {
		printf("buf info: %d. length:%d, ptr:%p\n",
		       i, buf.length, (void*)buf.m.userptr);
	    }

	    if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
		errno_exit ("VIDIOC_QBUF");
	}

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
	    errno_exit ("VIDIOC_STREAMON");

	break;
    }
	
    gettimeofday(&start_time, NULL);

}

static void
uninit_device                   (void)
{
    unsigned int i;

    switch (io) {
    case IO_METHOD_READ:
	free (buffers[0].start);
	break;

    case IO_METHOD_MMAP:
	for (i = 0; i < n_buffers; ++i)
	    if (-1 == munmap (buffers[i].start, buffers[i].length))
		errno_exit ("munmap");
	break;

    case IO_METHOD_USERPTR:
	for (i = 0; i < n_buffers; ++i)
	    free (buffers[i].start);
	break;
    }

    free (buffers);
}

static void
init_read			(unsigned int		buffer_size)
{
    buffers = calloc (1, sizeof (*buffers));

    if (!buffers) {
	fprintf (stderr, "Out of memory\n");
	exit (EXIT_FAILURE);
    }

    buffers[0].length = buffer_size;
    buffers[0].start = malloc (buffer_size);

    if (!buffers[0].start) {
	fprintf (stderr, "Out of memory\n");
	exit (EXIT_FAILURE);
    }
}

static void
init_mmap			(void)
{
    struct v4l2_requestbuffers req;

    CLEAR (req);

    req.count               = 4;
/*     req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE; */
/*     req.memory              = V4L2_MEMORY_MMAP; */
    req.type                = 1;
    req.memory              = 1;

    if (-1 == xioctl (fd, VIDIOC_REQBUFS, (char*)&req)) {
	if (EINVAL == errno) {
	    fprintf (stderr, "%s does not support "
		     "memory mapping\n", dev_name);
	    exit (EXIT_FAILURE);
	} else {
	    errno_exit ("VIDIOC_REQBUFS");
	}
    }

    if (req.count < 2) {
	fprintf (stderr, "Insufficient buffer memory on %s\n",
		 dev_name);
	exit (EXIT_FAILURE);
    }

    buffers = calloc (req.count, sizeof (*buffers));

    if (!buffers) {
	fprintf (stderr, "Out of memory\n");
	exit (EXIT_FAILURE);
    }

    for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
	struct v4l2_buffer buf;

	CLEAR (buf);

	buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory      = V4L2_MEMORY_MMAP;
	buf.index       = n_buffers;

	if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
	    errno_exit ("VIDIOC_QUERYBUF");

	buffers[n_buffers].length = buf.length;
	buffers[n_buffers].start =
	    mmap (NULL /* start anywhere */,
		  buf.length,
		  PROT_READ | PROT_WRITE /* required */,
		  MAP_SHARED /* recommended */,
		  fd, buf.m.offset);

	if (MAP_FAILED == buffers[n_buffers].start)
	    errno_exit ("mmap");
    }
}

static void
init_userp			(unsigned int		buffer_size)
{
    struct v4l2_requestbuffers req;
    unsigned int page_size;
    int i;

    page_size = getpagesize ();
    buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);

    CLEAR (req);

    req.count               = 4;
    req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory              = V4L2_MEMORY_USERPTR;

    if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
	if (EINVAL == errno) {
	    fprintf (stderr, "%s does not support "
		     "user pointer i/o\n", dev_name);
	    exit (EXIT_FAILURE);
	} else {
	    errno_exit ("VIDIOC_REQBUFS");
	}
    }

    buffers = calloc (4, sizeof (*buffers));

    if (!buffers) {
	fprintf (stderr, "Out of memory\n");
	exit (EXIT_FAILURE);
    }

    for (i  = 0; i < n_buffers ; ++i) {
	buffers[i].length = buffer_size;
/* 	buffers[n_buffers].start = memalign (/\* boundary *\/ page_size, */
/* 					     buffer_size); */
/* 	printf("!!!!!!!!!!!!!!!!!!! -- %d \n", page_size); */
 	buffers[i].start = malloc(604*1024); 
	unsigned long addr = buffers[i].start;
	addr = (addr+4095) & 0xfffff000;
	buffers[i].start = addr;
	if (!buffers[i].start) {
	    fprintf (stderr, "Out of memory\n");
	    exit (EXIT_FAILURE);
	}
	memset (buffers[i].start, 0, buffer_size);
	/*
	 * a memset let kernel do real alloc pte!
	 * don't move this !
	 *
	 * */
	if (verbose) {
	    printf("buffer alloc :%p =  %d \n",
		   buffers[i].start,
		   buffers[i].length);
	}

    }
}

static void enumlate_types()
{
    int res = 0;
    int i;
    for (i = 0; ; i++) {
	struct v4l2_fmtdesc fdesc;
	memset (&fdesc, 0, sizeof(fdesc));
	fdesc.index = i;
	fdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	res = ioctl (fd, VIDIOC_ENUM_FMT, &fdesc);
	if (res == -1) { printf ("end\n"); break; }
	else printf("%d. Support, %s \n",fdesc.index ,fdesc.description);
    }
}


static void enumlate_all_ctrl() 
{
    enumlate_types();
    struct v4l2_queryctrl ctrl;
    struct v4l2_querymenu menu;
    int i;

    for (i = V4L2_CID_BASE;i < V4L2_CID_LASTP1 ; i++) {
	CLEAR(ctrl);
	ctrl.id = i;
	int res = ioctl(fd, VIDIOC_QUERYCTRL, &ctrl);
	if (res == -1) break;
	printf("%2d. %15s default:%3d max:%3d min:%3d step:%3d\n",
	       i, ctrl.name, ctrl.default_value, 
	       ctrl.maximum, 
	       ctrl.minimum,
	       ctrl.step);
    }

    for (i = 0;; i++) {
	CLEAR(menu);
	menu.index = i;
	int res = ioctl (fd, VIDIOC_QUERYMENU, &menu);
	if (res == -1) break;
	printf("%2d. %s \t\tid:%d\n", menu.index, menu.name, menu.id);
    }

    struct v4l2_capability cap;
    CLEAR(cap);
    int res = ioctl (fd, VIDIOC_QUERYCAP, &cap);
    if (res == -1) { perror ( "VIDIOC_QUERYCAP error");return ;}
    printf("Driver :%s \nCard :%s \nbus info :%s \n",
	   cap.driver,cap.card,cap.bus_info);

#define detect_cap(name) do {						\
	if (cap.capabilities & name ) printf("support :%s \n", #name);	\
    } while (0)
		  
    detect_cap(V4L2_CAP_VIDEO_CAPTURE);
    detect_cap(V4L2_CAP_VIDEO_OUTPUT);
    detect_cap(V4L2_CAP_VIDEO_OVERLAY);
    detect_cap(V4L2_CAP_VBI_CAPTURE);
    detect_cap(V4L2_CAP_VBI_OUTPUT);
    detect_cap(V4L2_CAP_SLICED_VBI_CAPTURE);
    detect_cap(V4L2_CAP_SLICED_VBI_OUTPUT);
    detect_cap(V4L2_CAP_RDS_CAPTURE);
#ifdef V4L2_CAP_VIDEO_OUTPUT_OVERLAY
    
    detect_cap(V4L2_CAP_VIDEO_OUTPUT_OVERLAY);
#endif

}


static void
init_device (void)
{
    struct v4l2_capability cap;
    struct v4l2_cropcap cropcap;
    struct v4l2_crop crop;
    struct v4l2_format fmt;
    unsigned int min;


#ifdef ANDROID_OS
    int i ;
    for ( i = 0; i < 6; i++) {
	if  (-1 == xioctl (fd, VIDIOC_S_INPUT, &i)) {
	    if (EINVAL == errno ) 
		fprintf(stderr , "%s sensor type is not %d \n", dev_name, i);
	} else {
	    if (verbose)
		printf("success choose sensor %d \n", i);
	    break;
	}
    }
#endif

    if (enum_all) enumlate_all_ctrl(); 

    
    if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) {
	if (EINVAL == errno) {
	    fprintf (stderr, "%s is no V4L2 device\n",
		     dev_name);
	    exit (EXIT_FAILURE);
	} else {
	    errno_exit ("VIDIOC_QUERYCAP");
	}
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
	fprintf (stderr, "%s is no video capture device\n",
		 dev_name);
	exit (EXIT_FAILURE);
    }

    switch (io) {
    case IO_METHOD_READ:
	if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
	    fprintf (stderr, "%s does not support read i/o\n",
		     dev_name);
	    exit (EXIT_FAILURE);
	}

	break;

    case IO_METHOD_MMAP:
    case IO_METHOD_USERPTR:
	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
	    fprintf (stderr, "%s does not support streaming i/o\n",
		     dev_name);
	    exit (EXIT_FAILURE);
	}

	break;
    }

    /* Select video input, video standard and tune here. */

    CLEAR (cropcap);

    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
	crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	crop.c = cropcap.defrect; /* reset to default */

	if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
	    switch (errno) {
	    case EINVAL:
		/* Cropping not supported. */
		break;
	    default:
		/* Errors ignored. */
		break;
	    }
	}
    } else {	
	/* Errors ignored. */
    }


    CLEAR (fmt);
    _width = 640;
    _height = 480;

    fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width       = _width;
    fmt.fmt.pix.height      = _height;
    
/*         fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; */
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
/* #ifdef ANDROID_OS */
/*     fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG; */
/* #endif */
/* 	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24; */
    fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

    
    if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) 
	perror ("VIDIOC_S_FMT error, not support YUYV");

    /* try next 422P */
    if (direct_rgb)
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
    else 
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;

    if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) {
	perror ("S_FMT error on 422P!");
	exit (EXIT_FAILURE);
    }
	
    /* Note VIDIOC_S_FMT may change width and height. */

    /* Buggy driver paranoia. */
    min = fmt.fmt.pix.width * 2;
    if (fmt.fmt.pix.bytesperline < min)
	fmt.fmt.pix.bytesperline = min;
    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
    if (fmt.fmt.pix.sizeimage < min)
	fmt.fmt.pix.sizeimage = min;

    switch (io) {
    case IO_METHOD_READ:
	init_read (fmt.fmt.pix.sizeimage);
	break;

    case IO_METHOD_MMAP:
	init_mmap ();
	break;

    case IO_METHOD_USERPTR:
	init_userp (fmt.fmt.pix.sizeimage);
	break;
    }
}

static void
close_device(void)
{
    if (-1 == close (fd))
	errno_exit ("close");

    fd = -1;
}

static void
open_device(void)
{
    struct stat st; 

    if (-1 == stat (dev_name, &st)) {
	fprintf (stderr, "Cannot identify '%s': %d, %s\n",
		 dev_name, errno, strerror (errno));
	exit (EXIT_FAILURE);
    }

    if (!S_ISCHR (st.st_mode)) {
	fprintf (stderr, "%s is no device\n", dev_name);
	exit (EXIT_FAILURE);
    }

    fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);

    if (-1 == fd) {
	fprintf (stderr, "Cannot open '%s': %d, %s\n",
		 dev_name, errno, strerror (errno));
	exit (EXIT_FAILURE);
    }
}

static void
usage                           (FILE *                 fp,
                                 int                    argc,
                                 char **                argv)
{
    fprintf (fp,
	     "Usage: %s [options]\n\n"
	     "Options:\n"
	     "-d | --device name   Video device name [/dev/video0]\n"
	     "-h | --help          Print this message\n"
	     "-m | --mmap          Use memory mapped buffers\n"
	     "-r | --read          Use read() calls\n"
	     "-u | --userp         Use application allocated buffers\n"
	     "-j | --jpeg	       Output jpeg to /tmp directory \n"
	     "-e | --enum	       enumlate all ctrl and menu of device"
	     "",
	     argv[0]);
}

static const char short_options [] = "b:d:ejhmrvgun";

static const struct option
long_options [] = {
    { "fb",         required_argument,		NULL,	    'b' },
    { "device",     required_argument,      NULL,           'd' },
    { "help",       no_argument,            NULL,           'h' },
    { "mmap",       no_argument,            NULL,           'm' },
    { "read",       no_argument,            NULL,           'r' },
    { "userp",      no_argument,            NULL,           'u' },
    { "verbose",	no_argument,		NULL,		'v' },
    { "jpeg",	no_argument,		NULL,		'j' },
    { "enum",	no_argument,		NULL,		'e' },
    { "rgb",	no_argument,		NULL,		'g' },
    { "dry-run",no_argument,		NULL,		'n' },
    { 0, 0, 0, 0 }
};



static void set_parm () 
{
    struct v4l2_streamparm parm;
    parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    parm.parm.capture.timeperframe.denominator = 1*1000;
    parm.parm.capture.timeperframe.numerator = _camera_framerate*1000;
///----------------------------------------------
    parm.parm.capture.timeperframe.denominator = 0;
    parm.parm.capture.timeperframe.numerator = 0;

    /* _camera_framerate /  1 = framerate */
    parm.parm.capture.capturemode = 0;
    /*  1 is still image, high quality */
         
    if (ioctl(fd, VIDIOC_S_PARM, &parm) < 0)
    {
	perror("VIDIOC_S_PARM failed");
	exit(1);
    }
}

int
main (int argc,char **argv)
{
    dev_name = "/dev/video0";
    
    for (;;) {
	int index;
	int c;
                
	c = getopt_long (argc, argv,
			 short_options, long_options,
			 &index);

	if (-1 == c)
	    break;

	switch (c) {
	case 0: /* getopt_long() flag */
	    break;
	case 'b':
	    fb_dev = optarg;
	    break;
	case 'd':
	    dev_name = optarg;
	    break;

	case 'e':
	    enum_all = 1;
	    break;
	case 'h':
	    usage (stdout, argc, argv);
	    exit (EXIT_SUCCESS);

	case 'm':
	    io = IO_METHOD_MMAP;
	    break;
	case 'g':
	    direct_rgb = 1;
	    break;
	case 'r':
	    io = IO_METHOD_READ;
	    break;

	case 'u':
	    io = IO_METHOD_USERPTR;
	    break;
	case 'n':
	    dry_run = 1;
	    break;
	case 'j':
	    output_jpeg = 1;
	    break;

	case 'v':
	    verbose = 1;
	    break;


	default:
	    usage (stderr, argc, argv);
	    exit (EXIT_FAILURE);
	}
    }


    if (!output_jpeg) {
	test_file_fd = open("/tmp/test.yuv", O_RDWR | O_CREAT);
	if (test_file_fd < 0) {
	    perror ("test file open error ");
	    return 1;
	}
	/* TODO: this header 's frame rate must be set after
	 * the record is finished , but in very early time
	 * time this time , we just don't know this
	 * */
	char *str = "YUV4MPEG2 W%d H%d F30000:1001 Ip A10:11 C422\nFRAME\n";
	char buf[strlen(str) + 10];
	sprintf(buf, str, _width, _height);
	write (test_file_fd, buf, sizeof(buf));
    }

    open_device ();

    init_device ();

    set_parm();

    start_capturing ();

    mainloop ();

    stop_capturing ();

    uninit_device ();

    close_device ();

    exit (EXIT_SUCCESS);

    return 0;
}
